/* Modified from Altera bmp.c */
#include <string.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>

#include <config.h>

#ifdef BMP_SUPPORT

#include <systems.h>
#include <image_bmp.h>
#include <gapi.h>
#include <image.h>
//#define BMP_DEBUG
#undef DPRINTF
#ifdef BMP_DEBUG
#define DPRINTF(str, args...)   fprintf(stderr, "DEBUG: " str, ##args)  /* debug output*/
#else
#define DPRINTF(str, ...)
#endif

extern u32 xres, yres, bpp;

static void bmp_fskip(FILE *fp, int num_bytes)
{
    int i;
    for (i=0; i<num_bytes; i++)
        bmp_fgetc(fp);
}

BMP *load_bmp(u8 *file)
{
    FILE *fp;
    BMP *b;
    long index;
    int x, byte_count, temp, i;
    short color_index;
    u8 red, green, blue;
#if defined(CS_RGB565)
    u16 color;
#elif defined(CS_RGB888)
    u8 color;
#endif

    /* Make sure filename string is termintated */
    file[strlen(file)] = 0x0;

    /* open the file */
    if ((fp = bmp_fopen(file, "rb")) == NULL)
    {
        ERROR("Error opening file %s.\n", file);
        return 0;
    }

    /* check to see if it is a valid bitmap file */
    if (bmp_fgetc(fp)!='B' || bmp_fgetc(fp)!='M')
    {
        bmp_fclose(fp);
        ERROR("%s is not a bitmap file.\n", file);
        return 0;
    }
    
    b = (BMP *)malloc(sizeof(BMP));
    b->direct_write_to_fb = false;
    
    /* read in the width and height of the image, and the
       number of colors used; ignore the rest */
    bmp_fskip(fp,16);
    bmp_fread(&b->biWidth, sizeof(u32), 1, fp);
    bmp_fread(&b->biHeight,sizeof(u32), 1, fp);
    bmp_fskip(fp,2);
    bmp_fread(&b->biBitCount,sizeof(u16), 1, fp);
    bmp_fskip(fp,24);
  
    DPRINTF("BMP->Width: %d\n", b->biWidth);
    DPRINTF("BMP->Height: %d\n", b->biHeight);
    DPRINTF("BMP->BitCount: %d\n",b->biBitCount);

    /* try to allocate memory for palette data */
    if (b->biBitCount != 24)
    {
        if ((b->bicolor_palatte = (u8 *) malloc((1 << (b->biBitCount))*3)) == NULL)
        {
            bmp_fclose(fp);
            ERROR("Error allocating memory for file %s.\n", file);
            return 0;
        }
    }

    if(!b->direct_write_to_fb) {
        /* try to allocate memory for bitmap data */
        b->data = (u8 *)malloc((b->biWidth * b->biHeight) * bpp);
        // b->data = image_pool_alloc(b->biWidth * b->biHeight * 2);

        if(b->data == NULL)
        {
            bmp_fclose(fp);
            ERROR("Error allocating memory for file %s.\n", file);
            return 0;
        }
        //DPRINTF("BMP Address: %x\n", b->data);
    }
    else if(b->data == NULL) {
          DPRINTF("Start address in framebuffer of bmp image is not set!\n");
          return 0;
    }

    /* read the palette information */
    if (b->biBitCount != 24)
    {
        for(index = 0; index < (1 << (b->biBitCount)); index++)
        {
            b->bicolor_palatte[(int)(index * 3 + 2)] = bmp_fgetc(fp);
            b->bicolor_palatte[(int)(index * 3 + 1)] = bmp_fgetc(fp);
            b->bicolor_palatte[(int)(index * 3 + 0)] = bmp_fgetc(fp);
            x = bmp_fgetc(fp);
        }
    }


    /* read an 8-bit bitmap */
    if (b->biBitCount == 8)
    {
        for(index=(b->biHeight - 1)* b->biWidth; index >= 0; index -= b->biWidth) {
            for(x=0;x<b->biWidth;x++)
            {
                color_index = (int)bmp_fgetc(fp);
                blue = b->bicolor_palatte[(color_index * 3) + 2];
                green = b->bicolor_palatte[(color_index * 3) + 1];
                red = b->bicolor_palatte[(color_index * 3) + 0];
#if defined(CS_RGB565)
#if defined(NIOS2_COLOR_TRANSFORM_SUPPORT)
                color = (u16)CT_RGB2RGB565(red, green, blue);
#else
                color = RGB2PIXEL565(red, green, blue);
#endif
                ((u16 *)(b->data))[index+x] = color;
#elif defined(CS_RGB888)
                ((u8 *)(b->data))[index+x] = red;
                ((u8 *)(b->data))[index+x+1] = green;
                ((u8 *)(b->data))[index+x+2] = blue;
#endif
            }
            i = 4 - b->biWidth%4;
            if(i != 4)
                bmp_fskip(fp, i);
        }
    }
    
    /* read a 24-bit bitmap */
    if (b->biBitCount == 24)
    {
        for(index=(b->biHeight - 1)* b->biWidth; index >= 0; index -= b->biWidth)
        {
            for(x=0, byte_count=0; x < b->biWidth; x++, byte_count += 3)
            {
                blue  = bmp_fgetc(fp);
                green = bmp_fgetc(fp);
                red = bmp_fgetc(fp);
#if defined(CS_RGB565)
#if defined(NIOS2_COLOR_TRANSFORM_SUPPORT)
                color = (u16)CT_RGB2RGB565(red, green, blue);
#else
                color = RGB2PIXEL565(red, green, blue);
#endif
                ((u16 *)(b->data))[index+x] = color;
#elif defined(CS_RGB888)
                ((u8 *)(b->data))[index+x] = red;
                ((u8 *)(b->data))[index+x+1] = green;
                ((u8 *)(b->data))[index+x+2] = blue;
#endif
            }
            for(;byte_count%4 != 0;)
            {
                temp = (char)bmp_fgetc(fp);
                ++byte_count;
            }
        }
    }
  
    if(b->biBitCount != 24)
          free(b->bicolor_palatte);
    bmp_fclose(fp);
    return b;
}

int save_bmp(IMAGE *img, const char *fname)
{
    FILE *fd;
    BMP_HEADER *hdr;
    
    if(!img || !fname) {
        ERROR("Invalid arguments\n");
        return -1;
    }
        
    hdr = (BMP_HEADER *)malloc(sizeof(BMP_HEADER));
        
    hdr->type = 0x4D42;
    hdr->size = sizeof(*hdr);
    hdr->offset = sizeof(*hdr);
    
    hdr->info.size = sizeof(hdr->info);
    hdr->info.width = img->width;
    hdr->info.height = img->height;
    hdr->info.bits = 24;
    hdr->info.planes = 1;
    hdr->info.compression = 0;
    hdr->info.imagesize = hdr->info.width * hdr->info.height * hdr->info.bits / 8;
    hdr->info.xresolution = 0;
    hdr->info.yresolution = 0;
    hdr->info.ncolours = 0;
    hdr->info.importantcolours = 0;
    
    hdr->size += hdr->info.imagesize;
    
    fd = bmp_fopen(fname, "w");
    if(fd < 0) {
        ERROR("Can't open bmp file %s\n", fname);
        return (int)fd;
    }
    
    bmp_fwrite(hdr, sizeof(*hdr), 1, fd);
    bmp_fwrite(img->data, hdr->info.imagesize, 1, fd);
    
    bmp_fclose(fd);
    
    free(hdr);
    
    return 0;
}

#endif /* BMP_SUPPORT */
